/*
 * @Description: Robot chassis control node
 * @Version: V1.0
 * @Author: zw_1520@163.com
 * @Date: 2025-08-16 09:48:17
 * @LastEditors: zw_1520@163.com
 * @LastEditTime: 2025-08-21 11:39:01
 * Copyright (C) 2024-2050 Lens All rights reserved.
 */

#include "robot_chassis.h"

/**
 * @description: robot chassis constructor
 * @return {*}
 * @author: zw_1520@163.com
 */
ChassisController::ChassisController() : Node("robot_chassis") {
  m_chassis_kinematics_ptr_ = new ChassisKinematics();

  m_last_calculate_time_ = std::chrono::high_resolution_clock::now();

  m_chassis_joint_state_control_sub_ =
      this->create_subscription<sensor_msgs::msg::JointState>(
          "/chassis_joint_state", 10,
          std::bind(&ChassisController::chassisJointStateCallback, this, _1));

  m_chassis_velocity_control_sub_ =
      this->create_subscription<geometry_msgs::msg::Twist>(
          "/cmd_vel", 10,
          std::bind(&ChassisController::cmdVelCallback, this, _1));

  m_odom_publisher_ =
      this->create_publisher<nav_msgs::msg::Odometry>("/odom", 10);

  m_tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);

  m_odom_update_timer_ = this->create_wall_timer(
      std::chrono::milliseconds(20),
      std::bind(&ChassisController::odomUpdateTimerCallback, this));

  // Gets chassis configuration
  this->declare_parameter("wheel_distance", 0.5287);
  this->declare_parameter("wheel_diameter", 0.173);

  this->get_parameter("wheel_distance",
                      m_chassis_kinematics_ptr_->m_wheel_distance_);
  this->get_parameter("wheel_diameter",
                      m_chassis_kinematics_ptr_->m_wheel_diameter_);
  std::thread(&ChassisKinematics::timer, m_chassis_kinematics_ptr_).detach();
  RCLCPP_INFO(this->get_logger(), "wheel_distance: %f",
              m_chassis_kinematics_ptr_->m_wheel_distance_);
  RCLCPP_INFO(this->get_logger(), "wheel_diameter: %f",
              m_chassis_kinematics_ptr_->m_wheel_diameter_);
}

/**
 * @description: process chassis motor joint state
 * @return {*}
 * @author: zw_1520@163.com
 */
void ChassisController::chassisJointStateCallback(
    const sensor_msgs::msg::JointState::SharedPtr msg) const {
  RCLCPP_INFO(this->get_logger(), "chassis joint state callback, msg: %s",
              msg->header.frame_id.c_str());
}

/**
 * @description: chassis velocity control
 * @return {*}
 * @author: zw_1520@163.com
 */
void ChassisController::cmdVelCallback(
    const geometry_msgs::msg::Twist::SharedPtr msg) const {
  m_chassis_kinematics_ptr_->doubleAckermannIK(msg->linear.x, msg->angular.z);
  return;
}

/**
 * @description: update chassis odometry
 * @return {*}
 * @author: zw_1520@163.com
 */
void ChassisController::odomUpdateTimerCallback() { return; }
